#include <ament_index_cpp/get_package_share_directory.hpp>
#include <rclcpp/duration.hpp>
#include <rclcpp/qos.hpp>
#include <tf2_geometry_msgs/tf2_geometry_msgs.hpp>

#include <cv_bridge/cv_bridge.hpp>
#include <opencv2/core.hpp>
#include <opencv2/imgproc.hpp>

// 标准库
#include <algorithm>
#include <memory>
#include <string>
#include <vector>

// 自定义模块
#include "hero_integrated_vision/armor.hpp"
#include "hero_integrated_vision/hero_vision_node.hpp"
#include "hero_integrated_vision/traditional_detector.hpp"  

namespace rm_aim
{
// -------------------------- 策略类实现 --------------------------
// 关闭自瞄策略：无处理逻辑
void HeroVisionNode::CloseAimStrategy::doProcess(const sensor_msgs::msg::Image::ConstSharedPtr& img_msg) {}

// 普通自瞄策略：检测装甲板+发布消息
void HeroVisionNode::NormalAimStrategy::doProcess(const sensor_msgs::msg::Image::ConstSharedPtr& img_msg) 
{
  auto armors = node_.detectArmors(img_msg);
  node_.armors_msg_ = node_.cameraparams_manager_->generateArmorsMsg(armors, img_msg->header);
  node_.armors_pub_->publish(node_.armors_msg_);
}

// 前哨站策略：复用装甲板检测逻辑（可独立扩展）
void HeroVisionNode::OutpostStrategy::doProcess(const sensor_msgs::msg::Image::ConstSharedPtr& img_msg) 
{
  auto armors = node_.detectArmors(img_msg);
  node_.armors_msg_ = node_.cameraparams_manager_->generateArmorsMsg(armors, img_msg->header);
  node_.armors_pub_->publish(node_.armors_msg_);
}

// 绿灯策略：检测绿灯+生成消息+发布
void HeroVisionNode::GreenLightStrategy::doProcess(const sensor_msgs::msg::Image::ConstSharedPtr& img_msg) 
{
  
}

HeroVisionNode::HeroVisionNode(const rclcpp::NodeOptions & options)
: Node("hero_vision_node", options), current_strategy_(nullptr)
{
  RCLCPP_INFO(this->get_logger(), "Starting Hero Vision Node...");

  // 初始化装甲板检测器
  traditional_detector = initDetector();
  if (!traditional_detector || !traditional_detector->isReady()) {
    RCLCPP_ERROR(this->get_logger(), "Detector initialization failed, node startup aborted!");
    return;
  }

  // 创建装甲板检测结果发布者
  armors_pub_ = this->create_publisher<aim_interfaces::msg::Armors>(
    "/hero_vision/armors", rclcpp::SensorDataQoS());
  
  // 创建装甲板检测结果图片发布者
  result_img_pub_ = image_transport::create_publisher(
    this,
    "/hero_vision/result_image",  
    rmw_qos_profile_sensor_data    
  );

  // 初始化CameraParamsManager
  cameraparams_manager_ = std::make_unique<CameraParamsManager>(this);

  // 注册所有策略
  strategy_map_["close_aim"] = std::make_unique<CloseAimStrategy>(*this);
  strategy_map_["normal_aim"] = std::make_unique<NormalAimStrategy>(*this);
  strategy_map_["outpost"] = std::make_unique<OutpostStrategy>(*this);
  strategy_map_["green_light"] = std::make_unique<GreenLightStrategy>(*this);

  // 设置默认策略（普通自瞄）
  current_strategy_ = strategy_map_["normal_aim"].get();

  // 订阅控制指令
  control_sub_ = this->create_subscription<aim_interfaces::msg::GimbalControl>(
    "/gimbal/control_cmd", 10,
    [this](const aim_interfaces::msg::GimbalControl::ConstSharedPtr msg) {
      // 校验模式合法性
      std::vector<std::string> valid_modes = {"normal_aim", "outpost", "green_light", "close_aim"};
      if (std::find(valid_modes.begin(), valid_modes.end(), msg->mode) == valid_modes.end()) {
          RCLCPP_WARN(this->get_logger(), "Invalid mode: %s, keep current mode", msg->mode.c_str());
          return;
      }
      // 让CameraParamsManager更新倍镜模式状态
      this->cameraparams_manager_->updateScopeEnableState(msg->use_scope);
      // 切换当前策略
      this->switchStrategy(msg->mode);

      RCLCPP_INFO(this->get_logger(), "Update control cmd: mode=%s, use_scope=%s",msg->mode.c_str(), msg->use_scope ? "true" : "false");
    });

  // 订阅相机图像
  img_sub_ = this->create_subscription<sensor_msgs::msg::Image>(
    "/image_raw", rclcpp::SensorDataQoS(),
    std::bind(&HeroVisionNode::imageCallback, this, std::placeholders::_1));

  RCLCPP_INFO(this->get_logger(), "Hero Vision Node initialization completed (default: mode= normal_aim)");
}

void HeroVisionNode::imageCallback(const sensor_msgs::msg::Image::ConstSharedPtr img_msg)
{  
  if (current_strategy_) {
    current_strategy_->doProcess(img_msg); // 调用当前策略的处理逻辑
  }
}

std::unique_ptr<BaseDetector> HeroVisionNode::initDetector()
{
  // 声明并获取二值化阈值参数（0~255）
  rcl_interfaces::msg::ParameterDescriptor bin_thres_desc;
  bin_thres_desc.integer_range.resize(1);
  bin_thres_desc.integer_range[0].from_value = 0;
  bin_thres_desc.integer_range[0].to_value = 255;
  bin_thres_desc.integer_range[0].step = 1;
  int binary_thres = this->declare_parameter("binary_threshold", 160, bin_thres_desc);

  // 声明并获取检测颜色参数（0-红色，1-蓝色）
  rcl_interfaces::msg::ParameterDescriptor color_desc;
  color_desc.description = "Target color to detect (0-Red team, 1-Blue team)";
  color_desc.integer_range.resize(1);
  color_desc.integer_range[0].from_value = 0;
  color_desc.integer_range[0].to_value = 1;
  color_desc.integer_range[0].step = 1;
  int detect_color = this->declare_parameter("detect_color", RED_CAMP, color_desc);

  // 声明并获取灯条参数
  LightParams light_params;
  light_params.min_aspect_ratio = this->declare_parameter("light.min_aspect_ratio", 0.1);
  light_params.max_aspect_ratio = this->declare_parameter("light.max_aspect_ratio", 0.4);
  light_params.max_vertical_tilt_angle = this->declare_parameter("light.max_vertical_tilt_angle", 40.0);

  // 声明并获取装甲板参数
  ArmorParams armor_params;
  armor_params.min_light_length_ratio = this->declare_parameter("armor.min_light_length_ratio", 0.7);
  armor_params.min_small_armor_center_dist = this->declare_parameter("armor.min_small_center_dist", 0.8);
  armor_params.max_small_armor_center_dist = this->declare_parameter("armor.max_small_center_dist", 3.2);
  armor_params.min_large_armor_center_dist = this->declare_parameter("armor.min_large_center_dist", 3.2);
  armor_params.max_large_armor_center_dist = this->declare_parameter("armor.max_large_center_dist", 5.5);
  armor_params.max_horizontal_tilt_angle = this->declare_parameter("armor.max_horizontal_tilt_angle", 35.0);

  // 忽略的类别
  std::vector<std::string> ignore_classes = this->declare_parameter(
      "ignore_classes", std::vector<std::string>{"negative"});
  // 分类器阈值
  double classifier_threshold = this->declare_parameter("classifier_threshold", 0.7);
  // 创建传统检测器实例
  auto detector = std::make_unique<TraditionalDetector>(
    this->get_logger(),  // 日志器
    armor_params,       // 装甲板参数
    light_params,        // 灯条参数
    classifier_threshold,  // 置信度阈值
    binary_thres,          // 二值化阈值
    detect_color,       // 检测颜色
    ignore_classes      // 忽略的类别
  );

  // 初始化数字分类器（加载模型）
  try {
    std::string pkg_path = ament_index_cpp::get_package_share_directory("hero_integrated_vision");
    std::string model_path = pkg_path + "/models/mlp.xml";  // 分类器模型路径
    std::string label_path = pkg_path + "/models/label.txt";  // 标签文件路径

    // 初始化分类器
    if (!detector->init(model_path, label_path, "CPU")) { 
      RCLCPP_ERROR(this->get_logger(), "Number classifier model loading failed");
      return nullptr;
    }
    RCLCPP_INFO(this->get_logger(), "Number classifier initialized successfully");
  } catch (const std::exception & e) {
    RCLCPP_ERROR(this->get_logger(), "Classifier initialization failed: %s", e.what());
    return nullptr;
  }

  return detector;
}

std::vector<Armor> HeroVisionNode::detectArmors(const sensor_msgs::msg::Image::ConstSharedPtr & img_msg)
{
  // 将ROS图像消息转换为OpenCV格式（RGB8）
  cv_bridge::CvImageConstPtr cv_ptr;
  try {
    cv_ptr = cv_bridge::toCvShare(img_msg, "rgb8");
  } catch (cv_bridge::Exception & e) {
    RCLCPP_ERROR(this->get_logger(), "Image format conversion failed: %s", e.what());
    return {};
  }
  cv::Mat img = cv_ptr->image;

  // 调用检测器进行装甲板检测
  std::vector<Armor> armors = traditional_detector->detect(img);

  // 检测并绘制结果图像
  traditional_detector->drawArmorDetectionResults(img);
  result_img_pub_.publish(cv_bridge::CvImage(img_msg->header, "rgb8", img).toImageMsg());  // 发布结果图像
  return armors;
}

void HeroVisionNode::switchStrategy(const std::string& mode)
{
  auto it = strategy_map_.find(mode);
  if (it != strategy_map_.end()) {
    current_strategy_ = it->second.get();
    RCLCPP_DEBUG(this->get_logger(), "Switched to mode: %s", mode.c_str());
  } else {
    RCLCPP_WARN(this->get_logger(), "Invalid mode: %s (keep current mode)", mode.c_str());
  }
}


std::vector<cv::Rect2f> HeroVisionNode::detectGreenLights(const cv::Mat& img)
{
   return std::vector<cv::Rect2f>();  
}


void HeroVisionNode::drawGreenLightResults(cv::Mat& img, const std::vector<cv::Rect2f>& green_lights)
{

}

}  // namespace rm_aim

// 注册节点，使其可被ROS 2组件加载器发现
#include "rclcpp_components/register_node_macro.hpp"
RCLCPP_COMPONENTS_REGISTER_NODE(rm_aim::HeroVisionNode)